In [24]:

    

from IPython.core.display import display_html
from urllib.request import urlopen

link = 'https://gist.github.com/robblack007/eca03fa9f7586860235d/raw/ef05a2f29febc94a9c9f99ca20fd0b65e74ed451/custom.css'
display_html(urlopen(link).read(), raw=True)


    

In [2]:

    

from sympy import var, sin, cos, Matrix, Integer, eye, Function, Rational, exp, Symbol, I, solve, pi, trigsimp, dsolve, sinh, cosh, simplify
from sympy.physics.mechanics import mechanics_printing
mechanics_printing()


    

In [13]:

    

var("m1 m2 J1 J2 L1 L2 l1 l2 t g")


    

    
Out[13]:





$$\left ( m_{1}, \quad m_{2}, \quad J_{1}, \quad J_{2}, \quad L_{1}, \quad L_{2}, \quad l_{1}, \quad l_{2}, \quad t, \quad g\right )$$

In [14]:

    

q1 = Function("q1")(t)
q2 = Function("q2")(t)


    

In [15]:

    

x1 = l1*cos(q1)
y1 = l1*sin(q1)
v1 = x1.diff("t")**2 + y1.diff("t")**2
v1.trigsimp()


    

    
Out[15]:





$$l_{1}^{2} \left(\dot{q}_{1}\right)^{2}$$

In [16]:

    

x2 = L1*cos(q1) + l2*cos(q1 + q2)
y2 = L1*sin(q1) + l2*sin(q1 + q2)
v2 = x2.diff("t")**2 + y2.diff("t")**2
v2.trigsimp()


    

    
Out[16]:





$$L_{1}^{2} \left(\dot{q}_{1}\right)^{2} + 2 L_{1} l_{2} \operatorname{cos}\left(q_{2}\right) \left(\dot{q}_{1}\right)^{2} + 2 L_{1} l_{2} \operatorname{cos}\left(q_{2}\right) \dot{q}_{1} \dot{q}_{2} + l_{2}^{2} \left(\dot{q}_{1}\right)^{2} + 2 l_{2}^{2} \dot{q}_{1} \dot{q}_{2} + l_{2}^{2} \left(\dot{q}_{2}\right)^{2}$$

In [17]:

    

ω1 = q1.diff("t")
ω2 = q2.diff("t")


    

In [18]:

    

K1 = Rational(1, 2)*m1*v1 + Rational(1, 2)*J1*ω1**2
K1


    

    
Out[18]:





$$\frac{J_{1} \left(\dot{q}_{1}\right)^{2}}{2} + \frac{m_{1}}{2} \left(l_{1}^{2} \operatorname{sin}^{2}\left(q_{1}\right) \left(\dot{q}_{1}\right)^{2} + l_{1}^{2} \operatorname{cos}^{2}\left(q_{1}\right) \left(\dot{q}_{1}\right)^{2}\right)$$

In [19]:

    

K2 = Rational(1, 2)*m1*v2 + Rational(1, 2)*J2*ω2**2
K2


    

    
Out[19]:





$$\frac{J_{2} \left(\dot{q}_{2}\right)^{2}}{2} + \frac{m_{1}}{2} \left(\left(- L_{1} \operatorname{sin}\left(q_{1}\right) \dot{q}_{1} - l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{1} + q_{2}\right)\right)^{2} + \left(L_{1} \operatorname{cos}\left(q_{1}\right) \dot{q}_{1} + l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{cos}\left(q_{1} + q_{2}\right)\right)^{2}\right)$$

In [20]:

    

U1 = m1*g*y1
U1


    

    
Out[20]:





$$g l_{1} m_{1} \operatorname{sin}\left(q_{1}\right)$$

In [21]:

    

U2 = m2*g*y2
U2


    

    
Out[21]:





$$g m_{2} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + l_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right)$$

In [22]:

    

K = K1 + K2
K


    

    
Out[22]:





$$\frac{J_{1} \left(\dot{q}_{1}\right)^{2}}{2} + \frac{J_{2} \left(\dot{q}_{2}\right)^{2}}{2} + \frac{m_{1}}{2} \left(l_{1}^{2} \operatorname{sin}^{2}\left(q_{1}\right) \left(\dot{q}_{1}\right)^{2} + l_{1}^{2} \operatorname{cos}^{2}\left(q_{1}\right) \left(\dot{q}_{1}\right)^{2}\right) + \frac{m_{1}}{2} \left(\left(- L_{1} \operatorname{sin}\left(q_{1}\right) \dot{q}_{1} - l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{1} + q_{2}\right)\right)^{2} + \left(L_{1} \operatorname{cos}\left(q_{1}\right) \dot{q}_{1} + l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{cos}\left(q_{1} + q_{2}\right)\right)^{2}\right)$$

In [23]:

    

U = U1 + U2
U


    

    
Out[23]:





$$g l_{1} m_{1} \operatorname{sin}\left(q_{1}\right) + g m_{2} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + l_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right)$$

In [24]:

    

L = (K - U).expand().simplify()
L


    

    
Out[24]:





$$\frac{J_{1} \left(\dot{q}_{1}\right)^{2}}{2} + \frac{J_{2} \left(\dot{q}_{2}\right)^{2}}{2} + \frac{L_{1}^{2} m_{1}}{2} \left(\dot{q}_{1}\right)^{2} - L_{1} g m_{2} \operatorname{sin}\left(q_{1}\right) + L_{1} l_{2} m_{1} \operatorname{cos}\left(q_{2}\right) \left(\dot{q}_{1}\right)^{2} + L_{1} l_{2} m_{1} \operatorname{cos}\left(q_{2}\right) \dot{q}_{1} \dot{q}_{2} - g l_{1} m_{1} \operatorname{sin}\left(q_{1}\right) - g l_{2} m_{2} \operatorname{sin}\left(q_{1} + q_{2}\right) + \frac{l_{1}^{2} m_{1}}{2} \left(\dot{q}_{1}\right)^{2} + \frac{l_{2}^{2} m_{1}}{2} \left(\dot{q}_{1}\right)^{2} + l_{2}^{2} m_{1} \dot{q}_{1} \dot{q}_{2} + \frac{l_{2}^{2} m_{1}}{2} \left(\dot{q}_{2}\right)^{2}$$

In [25]:

    

τ1 = (L.diff(q1.diff(t)).diff(t) - L.diff(q1)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))


    

In [26]:

    

τ2 = (L.diff(q2.diff(t)).diff(t) - L.diff(q2)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))


    

In [19]:

    

τ1


    

    
Out[19]:





$$g l_{1} m_{1} \operatorname{cos}\left(q_{1}\right) + g l_{1} m_{2} \operatorname{cos}\left(q_{1}\right) + g l_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right) - 2 l_{1} l_{2} m_{1} \operatorname{sin}\left(q_{2}\right) \dot{q}_{1} \dot{q}_{2} - l_{1} l_{2} m_{1} \operatorname{sin}\left(q_{2}\right) \left(\dot{q}_{2}\right)^{2} + \left(l_{1} l_{2} m_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}^{2} m_{1}\right) \ddot{q}_{2} + \left(2 l_{1}^{2} m_{1} + 2 l_{1} l_{2} m_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}^{2} m_{1}\right) \ddot{q}_{1}$$

In [20]:

    

τ2


    

    
Out[20]:





$$g l_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right) + l_{1} l_{2} m_{1} \operatorname{sin}\left(q_{2}\right) \left(\dot{q}_{1}\right)^{2} + l_{2}^{2} m_{1} \ddot{q}_{2} + \left(l_{1} l_{2} m_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}^{2} m_{1}\right) \ddot{q}_{1}$$

In [21]:

    

from scipy.integrate import odeint
from numpy import linspace


    

In [28]:

    

def pendulo_doble(estado, tiempo):
    # Se importan funciones necesarias
    from numpy import sin, cos, matrix
    # Se desenvuelven variables del estado y tiempo
    q1, q2, q̇1, q̇2 = estado
    t = tiempo
    
    # Se declaran constantes del sistema
    m1, m2 = 1, 1
    l1, l2 = 1, 1
    g = 9.81
    
    # Se declaran constantes del control
    kp1, kp2 = -30, -60
    kv1, kv2 = -20, -20
    
    # Señales de control nulas
    tau1, tau2 = 0, 0
    
    # Posiciones a alcanzar
    qd1, qd2 = 1, 1
    
    # Se declaran señales de control del sistema
    #tau1 = kp1*(q1 - qd1) + kv1*q̇1
    #tau2 = kp2*(q2 - qd2) + kv2*q̇2
    
    # Se calculan algunos terminos comunes
    ϕ1 = m1*l1**2
    ϕ2 = m1*l1*l2
    ϕ3 = m1*l2**2
    
    # Se calculan las matrices de masas, Coriolis,
    # y vectores de gravedad, control, posicion y velocidad
    M = matrix([[2*ϕ1 + 2*ϕ2*cos(q2) + ϕ3, ϕ2*cos(q2) + ϕ3],
                [ϕ2*cos(q2) + ϕ3, ϕ3]])
    C = matrix([[-2*ϕ2*sin(q2)*q̇2, -ϕ2*sin(q2)*q̇2], [ϕ2*sin(q2)*q̇1, 0]])
    G = matrix([[m1*l1*cos(q1) + m2*l1*cos(q1) + m2*l2*cos(q1 + q2)], [m2*l2*cos(q1 + q2)]])
    Tau = matrix([[tau1], [tau2]])
    q = matrix([[q1], [q2]])
    q̇ = matrix([[q̇1], [q̇2]])
    
    # Se calcula la derivada del estado del sistema
    qp1 = q̇1
    qp2 = q̇2
    
    qpp = M.I*(Tau - C*q̇ - G)
    qpp1, qpp2 = qpp.tolist()
    
    return [qp1, qp2, qpp1[0], qpp2[0]]


    

In [30]:

    

t = linspace(0, 10, 1000)
estados_simulados = odeint(func = pendulo_doble, y0  = [0, 0, 0, 0], t = t)


    

In [31]:

    

q1, q2, q̇1, q̇2 = list(zip(*estados_simulados.tolist()))


    

In [32]:

    

%matplotlib notebook
from matplotlib.pyplot import plot, style, figure
from mpl_toolkits.mplot3d import Axes3D
style.use("ggplot")


    

In [40]:

    

fig1 = figure(figsize=(8, 8))

ax1 = fig1.gca()

p1, = ax1.plot(t, q1)
p2, = ax1.plot(t, q2)
ax1.legend([p1, p2],[r"$q_1$", r"$q_2$"])
ax1.set_ylim(-4, 4)
ax1.set_xlim(-0.1, 10);


    

In [ ]: